/* CPSC 568: Group Assignment 1
 * iRobot Wall Measuring Madness
 * Authors: Michael Widmer and John Alvorado
 * Note: Some code taken from Rob Kremer's Line Follower class
 */

import java.text.ParseException;
import java.util.List;
import jade.semantics.lang.sl.grammar.Term;
import iRobotCreate.Controller;
import iRobotCreate.IRobotState;
import iRobotCreate.iRobotCommands;
import iRobotCreate.iRobotCommands.Sensor;
import casa.LispAccessible;
import casa.ML;
import casa.MLMessage;
import casa.PerformDescriptor;
import casa.URLDescriptor;
import casa.abcl.ParamsMap;
import casa.conversation2.SubscribeClientConversation;
import casa.event.TimeEvent;
import casa.ui.AgentUI;

//LOL WUT
public class myLittleiRobot extends Controller {
	
	int roboSpeed;
	int answer = 0;
	private boolean foundCorner = false;
	private boolean foundVirtWall = false;
	private boolean isDone = false;

	static {
		  createCasaLispOperators(myLittleiRobot.class);
	  }
	
	public myLittleiRobot(ParamsMap params, AgentUI ui) throws Exception {
		super(params, ui);
	}
	
	TimeEvent runTimer = null;
	@Override
	public void initializeAfterRegistered(boolean registered) {
		super.initializeAfterRegistered(registered);
		 
		try{
			
			@SuppressWarnings("unused")
			SubscribeClientConversation convBump = new SubscribeClientConversation(
					"--subscription-request", 
					this, getServer(), 
					"(all ?x (BumpsAndWheelDrops ?x))", null)
			{
				@Override
				public void update(URLDescriptor agentB, Term term) {
					if (term==null || demoOn())
						return;
					String intString = term.toString();
					int val = Integer.parseInt(intString);
					onBumpsAndWheelDrops(val);
				}
			};
				
			@SuppressWarnings("unused")
			SubscribeClientConversation convWall = new SubscribeClientConversation(
					"--subscription-request", 
					this, getServer(), 
					"(all ?x (Wall ?x))", null)
			{
				@Override
				public void update(URLDescriptor agentB, Term term) {
					if (term==null || demoOn())
						return;
					String intString = term.toString();
					int val = Integer.parseInt(intString);
					onWall(val);
				}
			};				
							
			@SuppressWarnings("unused")
			SubscribeClientConversation convVirtWall = new SubscribeClientConversation(
					"--subscription-request", 
					this, getServer(), 
					"(all ?x (VirtualWall ?x))", null)
			{
				@Override
				public void update(URLDescriptor agentB, Term term) {
					if (term==null || demoOn())
						return;
					String intString = term.toString();
					int val = Integer.parseInt(intString);
					onVirtWall(val);
				}
			};			
		}
		catch (Throwable e)
		{
			println("error", "Controller.initializeAfterRegistered().SubscribeConversation", e);
		}
		//Wag to show ready for fun stuff
		sendMessage(ML.REQUEST, ML.EXECUTE, getServer(),
				ML.LANGUAGE, "lisp",
				ML.CONTENT, "(progn () (irobot.drive 0 :flush T) (irobot.rotate-deg -30) )");
		sendMessage(ML.REQUEST, ML.EXECUTE, getServer(),
				ML.LANGUAGE, "lisp",
				ML.CONTENT, "(progn () (irobot.drive 0) (irobot.rotate-deg 30) )");
		//Create and start 10 minute timer
		runTimer = new TimeEvent(ML.EVENT, This, System.currentTimeMillis()+600000);
		runTimer.start();
		//Sets initial state
		setState(wandering);
	}
	
	IRobotState wandering = new IRobotState("wandering"){
		@Override public void enterState(){
			sendMessage(ML.REQUEST, ML.EXECUTE, getServer() 
					  ,ML.LANGUAGE, "lisp"
					  ,ML.CONTENT, "(irobot.drive 300)"
					  );
			foundVirtWall = false;
			foundCorner = false;
			isDone = false;
		}
		@Override public void handleEvent(Sensor sensor, short reading){
			//System.out.println("Wandering State");
			switch(sensor){
			case BumpsAndWheelDrops:
				int deg = 0;
				switch (reading & 3) { //determine which bump sensor(s) were triggered (if any)
				case 0: // no bumps
					break;
				case 1: // right
					sendMessage(ML.REQUEST, ML.EXECUTE, getServer() 
							,ML.LANGUAGE, "lisp"
							,ML.CONTENT, "(progn () (irobot.drive 0) (irobot.moveby -25) )"
							);
					setState(aligning);
					break;
				case 2: // left
					deg = -35;
					break;
				case 3: // both
					deg = 70;
					break;
				default:
					break;
				}
				// back up a bit to clear the obstacle and make the appropriate turn, and continue
				if (deg!=0) {
					roboSpeed = 150;
					sendMessage(ML.REQUEST, ML.EXECUTE, getServer() 
							,ML.LANGUAGE, "lisp"
							,ML.CONTENT, "(progn () (irobot.drive 0 :flush T) (irobot.moveby -50) (irobot.rotate-deg "+deg+") (irobot.drive "+roboSpeed+"))"
							);
				}
				break;	
			default:
				break;
			}
		}
	};
	
	final static int chasisPerimeter = (int)(iRobotCommands.chassisDiameter*3.14);
	myLittleiRobot This = this;
	TimeEvent alignSpinTimer = null;
	
	IRobotState aligning = new IRobotState("aligning"){
		@Override public void enterState(){
			int spinSpeed = defSpeed/2;
			int timeToCircle = (chasisPerimeter/spinSpeed)*1000;
			alignSpinTimer = new TimeEvent(ML.EVENT, This, System.currentTimeMillis()+timeToCircle);
			alignSpinTimer.start();
			//System.out.println("Align state.");
			sendMessage(ML.REQUEST, ML.EXECUTE, getServer(), ML.LANGUAGE, "lisp", ML.CONTENT, "(irobot.drive "+(short)(spinSpeed)+" "+(short)iRobotCommands.P_TurnInPlaceCounterClockwise+")");
			// Turn Counterclockwise until wall disappears, or timer runs out.
		}
		@Override
		public void handleEvent(Sensor sensor, short reading) {
			switch(sensor){
				case Wall:
					switch (reading) {
					case 0: //No wall
						//System.out.println("Aligning state - Does not see wall anymore.");
						alignSpinTimer.cancel();
						sendMessage(ML.REQUEST, ML.EXECUTE, getServer(),
								ML.LANGUAGE, "lisp",
								ML.CONTENT, "(progn () (irobot.drive 0 :flush T) (irobot.rotate-deg -30) )");
						//System.out.println("Ready for further commands, master.");
						setState(followTheWall);
						break;
					case 1: //Wall found
						//System.out.println("Wall ahoy!");
						break;
					}
					break;
				default:
					break;						
			}
		}		
	};
	
	IRobotState followTheWall = new IRobotState("followTheWall"){
		@Override public void enterState(){
			//System.out.println("Follow wall state.");
			sendMessage(ML.REQUEST, ML.EXECUTE, getServer(),
					ML.LANGUAGE, "lisp",
					ML.CONTENT, "(irobot.drive "+defSpeed+")");
		}
		@Override
		public void handleEvent(Sensor sensor, short reading) {
			switch(sensor){
				case Wall:
					switch (reading) {
					case 0:
						setState(realignToWall);
						break;
					case 1: //Wall sensor still detects the wall present 
						//System.out.println("followTheWall Case 1");
						sendMessage(ML.REQUEST, ML.EXECUTE, getServer(),
								ML.LANGUAGE, "lisp",
								ML.CONTENT, "(irobot.drive "+defSpeed+" -10000)");
						break;
					}
					break;
				case BumpsAndWheelDrops:
					switch (reading & 3){
					case 1:					
						sendMessage(ML.REQUEST, ML.EXECUTE, getServer(),
								ML.LANGUAGE, "lisp",
								ML.CONTENT, "(progn () (irobot.drive 0 :flush T) (irobot.moveby -20) (irobot.rotate-deg 20) (irobot.drive "+defSpeed+"))");
						break;
					case 2:
						sendMessage(ML.REQUEST, ML.EXECUTE, getServer(),
								ML.LANGUAGE, "lisp",
								ML.CONTENT, "(progn () (irobot.drive 0 :flush T) (irobot.moveby -20) (irobot.rotate-deg -20) (irobot.drive "+defSpeed+"))");
						foundCorner = false;
					case 3:
						sendMessage(ML.REQUEST, ML.EXECUTE, getServer(),
								ML.LANGUAGE, "lisp",
								ML.CONTENT, "(progn () (irobot.drive 0 :flush T) (irobot.moveby -20) (irobot.rotate-deg 90) (irobot.drive "+defSpeed+"))");
						// Set FoundCorner flag
						foundCorner = true;
						// Get distanceAcc value from agent. Then calculate value if it's the correct wall.
						URLDescriptor otherAgent = new URLDescriptor(6900);						
						sendMessage(ML.QUERY_REF, null, otherAgent,
								ML.LANGUAGE, ML.FIPA_SL,
								ML.CONTENT, "(some ?x (distanceAcc ?x))"
								);
					}
					break;
				case VirtualWall:
					if (foundCorner){
						foundVirtWall = true;
					}
					break;
			default:
				break;
			
			}
		}		
	};
	
	//Reverses and realigns iRobot when wall disappears in followTheWall state
	TimeEvent reverseIt = null;
	IRobotState realignToWall = new IRobotState("realignToWall"){
		@Override
		public void enterState(){
			//System.out.println(getCurrentState().getName()+" - start NOW!");
			reverseIt = new TimeEvent(ML.EVENT, This, System.currentTimeMillis()+1500);
			reverseIt.start();
			sendMessage(ML.REQUEST, ML.EXECUTE, getServer(),
					ML.LANGUAGE, "lisp",
					ML.CONTENT, "(progn () (irobot.drive 0 :flush T) (irobot.drive -50))");
		}
		
		@Override
		public void handleEvent(Sensor sensor, short reading){
			switch(sensor){
			case Wall:
				switch (reading) {
				case 1:
					reverseIt.cancel();
					sendMessage(ML.REQUEST, ML.EXECUTE, getServer(),
							ML.LANGUAGE, "lisp",
							ML.CONTENT, "(progn () (irobot.drive 0 :flush T) (irobot.rotate-deg -15))");
					setState(followTheWall);
					break;					
				}
				break;
			case Overcurrents:
				//If overcurrent while backing up, move a forward a bit, turn right 90 deg, then go to align state to fix.
				//This case never happened while testing, so not sure if it will work yet.
				sendMessage(ML.REQUEST, ML.EXECUTE, getServer(),
						ML.LANGUAGE, "lisp",
						ML.CONTENT, "(progn () (irobot.drive 0 :flush T) (irobot.moveby 20) (irobot.rotate-deg -90))");
				reverseIt.cancel();
				setState(aligning);
			default:
				break;
			}
		}
	};
	
	IRobotState waiting = new IRobotState("waiting"){
		@Override
		public void enterState(){
			if (isDone) {
				sendMessage(ML.REQUEST, ML.EXECUTE, getServer(),
						ML.LANGUAGE, "lisp",
						ML.CONTENT, "(progn () (irobot.drive 0 :flush T) (irobot.moveby -20))");
			} else {
				sendMessage(ML.REQUEST, ML.EXECUTE, getServer(),
						ML.LANGUAGE, "lisp",
						ML.CONTENT, "(irobot.drive 0 :flush T)");
			}
		}
		
		@Override
		public void handleEvent(Sensor sensor, short reading) {			
		}		
	};
	
	@Override
	public PerformDescriptor release_query_ref(MLMessage message)
	{
		List<Integer> distList;
		PerformDescriptor value = super.release_query_ref(message);
		try {
			//Get value of distanceAcc from agent
			distList = queryRef_toIntegerList("(some ?x (distanceAcc ?x))");
			if(foundVirtWall && !isDone)
			{
				isDone = true;
				calculateDistance(distList);
			}
			
		} catch (ParseException e) {
			e.printStackTrace();
		}		
		return value;
	}
	
	public void calculateDistance(List<Integer> distList){
		//Tracy Luu showed us the command to output this onto the UI.
		//Gets difference of the last two values between corners
		answer = distList.get(distList.size()-1) - distList.get(distList.size()-2);
		answer += iRobotCommands.chassisDiameter;
		AgentUI resultMeasurement = this.getUI();
		resultMeasurement.println("The length of THE WALL is: "+answer+"mm.");
		runTimer.cancel();
		setState(waiting);
	}
	
	//Initiates wandering state to begin measure algorithm
	@LispAccessible	(
			name = "measure"
			)
	public void measure(){
		answer = 0;
		setState(wandering);
	}	
	
	//Reports length of state and wall length, if found. Otherwise, only state.
	@LispAccessible(
			name = "report"	
			)
	public void report(){
		AgentUI thatAgent = this.getUI();		
		thatAgent.println("My current state is: "+getCurrentStateName());
		if(answer != 0)
			thatAgent.println("Length is "+answer);
		else
			thatAgent.println("Length has not yet been calculated. So solly.");		
	}	
	
	//Stops iRobot from whatever it's doing and resets to fresh start
	@LispAccessible(
			name = "reset"
			)
	public void reset(){
		answer = 0;
		sendMessage(ML.REQUEST, ML.EXECUTE, getServer(),
				ML.LANGUAGE, "lisp",
				ML.CONTENT, "(irobot.drive 0 :flush T)");
		setState(waiting);
	}	
	
	//Stops iRobot from whatever it's doing and turns 180 degrees
	@LispAccessible(
			name = "aboutHut"
			)
	public void aboutHut() {
		sendMessage(ML.REQUEST, ML.EXECUTE, getServer(),
				ML.LANGUAGE, "lisp",
				ML.CONTENT, "(progn () (irobot.drive 0 :flush T) (irobot.moveby -10) (irobot.rotate-deg -180))");
	}
	
	@Override
	protected void onBumpsAndWheelDrops(int val) {
 		getCurrentState().handleEvent(Sensor.BumpsAndWheelDrops, (short)val);
	}
		
	protected void onWall(int val){
		getCurrentState().handleEvent(Sensor.Wall, (short)val);
	}

	protected void onVirtWall(int val){
		getCurrentState().handleEvent(Sensor.VirtualWall, (short)val);
	}
}